#!/usr/bin/python3
import pygame
import sys

import rospy
from geometry_msgs.msg import Twist

speed = 0.4
turn = 2

twist = Twist()
twist.linear.x = 0
twist.linear.y = 0
twist.linear.z = 0
twist.angular.x = 0
twist.angular.y = 0
twist.angular.z = 0

if __name__ == "__main__":
    rospy.init_node('turtlebot_teleop2')
    pub = rospy.Publisher('cmd_vel', Twist, queue_size=5)
    pub.publish(twist)
    
    pygame.init()
    s = pygame.display.set_mode((200, 200))
    pygame.display.set_caption("遥控器")

    print('\r\n\r\nwsad 前后左右 qe左右原地转 12减加速')
    print('请激活遥控器窗口以使能遥控器')
    print("currently:\tspeed %.2f\tturn %.2f " % (speed, turn))
    while True:
        event = pygame.event.wait()
        if event.type == pygame.QUIT:
            sys.exit()
        elif event.type == pygame.KEYDOWN:
            if event.unicode == 'w':
                twist.linear.x = speed
            elif event.unicode == 's':
                twist.linear.x = -speed
            elif event.unicode == 'a':
                twist.linear.y = speed
            elif event.unicode == 'd':
                twist.linear.y = -speed
            elif event.unicode == 'q':
                twist.angular.z = -turn
            elif event.unicode == 'e':
                twist.angular.z = turn
            elif event.unicode == '1':
                speed *= 0.6667
                turn *= 0.6667
                twist.linear.x *= 0.6667
                twist.linear.y *= 0.6667
                twist.linear.z *= 0.6667
                twist.angular.x *= 0.6667
                twist.angular.y *= 0.6667
                twist.angular.z *= 0.6667
                print("currently:\tspeed %.2f\tturn %.2f " % (speed, turn))
            elif event.unicode == '2':
                speed *= 1.5
                turn *= 1.5
                twist.linear.x *= 1.5
                twist.linear.y *= 1.5
                twist.linear.z *= 1.5
                twist.angular.x *= 1.5
                twist.angular.y *= 1.5
                twist.angular.z *= 1.5
                print("currently:\tspeed %.2f\tturn %.2f " % (speed, turn))
            elif event.unicode == "":
                print("#", event.key, event.mod)
            else:
                print(event.unicode, event.key, event.mod)
            pub.publish(twist)
        elif event.type == pygame.KEYUP:
            if event.unicode == 'w':
                twist.linear.x = 0
            elif event.unicode == 's':
                twist.linear.x = 0
            elif event.unicode == 'a':
                twist.linear.y = 0
            elif event.unicode == 'd':
                twist.linear.y = 0
            elif event.unicode == 'q':
                twist.angular.z = 0
            elif event.unicode == 'e':
                twist.angular.z = 0
            else:
                pass
            pub.publish(twist)

    # pygame.display.update()
